//用户任务SDK

#include "Ano_ProgramCtrl_User.h"
#include "Ano_Math.h"
#include "Ano_Pid.h"

#include "Ano_Parameter.h"
#include "Ano_FlightCtrl.h"
#include "Ano_LocCtrl.h"
#include "Ano_AttCtrl.h"

#include "Ano_T265.h"
#include "math.h"
#include "User_Program.h"

#define MAX_PC_XYVEL_CMPS  200
#define MAX_PC_ZVEL_CMPS   150
#define MAX_PC_PAL_DPS     100
#define Speed_Limit 20
#define ERR_LIMIT 5

_pc_user_st pc_user;
sdk_manager_t sdk_manager;


_PID_arg_st PID_X;
_PID_arg_st PID_Y;
_PID_arg_st PID_H;

_PID_val_st PID_X_Val;
_PID_val_st PID_Y_Val;
_PID_val_st PID_H_Val;



//=====1、航向水平坐标系程控速度功能接口函数=====
/**********************************************************************************************************
*函 数 名: Program_Ctrl_User_Set_HXYcmps
*功能说明: 程控功能，航向水平坐标系下速度设定（实时控制）
*参    数: X速度（厘米每秒，正为前进，负为后退，Y速度（厘米每秒，正为左移，负为右移）
*返 回 值: 无
**********************************************************************************************************/
void Program_Ctrl_User_Set_HXYcmps(float hx_vel_cmps,float hy_vel_cmps)
{
	//
	pc_user.vel_cmps_set_h[0] = hx_vel_cmps;
	pc_user.vel_cmps_set_h[1] = hy_vel_cmps;
	//限制XY速度模长
	length_limit(&pc_user.vel_cmps_set_h[0],&pc_user.vel_cmps_set_h[1],MAX_PC_XYVEL_CMPS,pc_user.vel_cmps_set_h);
	//pc_user设定值反映到FlightControl综合并输出到fs.speed_set_h[X]
}


//=====2、无头模式参考坐标系程控功速度能接口函数（暂无，稍后开发，或者参考上位机程控功能）=====
//
//
//

//=====3、通用程控速度功能接口函数=====
/**********************************************************************************************************
*函 数 名: Program_Ctrl_User_Set_WHZcmps
*功能说明: 程控功能，上升下降速度设定（实时控制）
*参    数: 速度（厘米每秒，正为上升，负为下降）
*返 回 值: 无
**********************************************************************************************************/
void Program_Ctrl_User_Set_Zcmps(float z_vel_cmps)
{
	//
	pc_user.vel_cmps_set_z = z_vel_cmps;
	//限幅
	pc_user.vel_cmps_set_z = LIMIT(pc_user.vel_cmps_set_z,-MAX_PC_ZVEL_CMPS,MAX_PC_ZVEL_CMPS);
}
/**********************************************************************************************************
*函 数 名: Program_Ctrl_User_Set_YAWdps
*功能说明: 程控功能，航向速度设定（实时控制）
*参    数: 速度（度每秒，正为右转，负为左转）
*返 回 值: 无
**********************************************************************************************************/
void Program_Ctrl_User_Set_YAWdps(float yaw_pal_dps)
{
	//
	pc_user.pal_dps_set = yaw_pal_dps;
	//限幅
	pc_user.pal_dps_set = LIMIT(pc_user.pal_dps_set,-MAX_PC_PAL_DPS,MAX_PC_PAL_DPS);
}

#define MAX_VELOCITY_X 70
#define MAX_VELOCITY_Y 70
#define MAX_ALT_THR 15
#define LAND_SPEED -25
#define LAND_TIME 900

typedef enum
{
    sdk_alt = 0,
    // sdk_velocity_x,
    // sdk_velocity_y,
    sdk_pos_x,
    sdk_pos_y,

    sdk_pid_list,
} sdk_pid_list_t;

uint8_t Rotation=1;





/******************************************************************************
 * 函数名称：sdk_velocity_reset
* 函数描述：重??水平速度
 * ??    入：void
 * ??    出：void
 * ??    回：void 
* ??    ??：相当于让???机空中??停静??
******************************************************************************/
void sdk_velocity_reset()
{
   pc_user.vel_cmps_set_h[0] = 0;
   pc_user.vel_cmps_set_h[1]= 0;
}

/******************************************************************************
 * 函数名称：sdk_velociyt_x_set
 * 函数描述：???定X轴的速度??
 * ??    入：float X 表示Y轴的速度设定??
 * ??    出：void
 * ??    回：void
 * ??    ??：单??设定x轴的速度??
******************************************************************************/
void sdk_velocity_x_set(float x)
{
    pc_user.vel_cmps_set_h[0] = LIMIT(x, -MAX_VELOCITY_X, MAX_VELOCITY_X);//限幅
    pc_user.vel_cmps_set_h[0]=Rotation*pc_user.vel_cmps_set_h[0];
}

/******************************************************************************
 * 函数名称：sdk_velociyt_y_set
* 函数描述：???定Y轴的速度??
* ??    入：float y 表示Y轴的速度设定??
 * ??    出：void
 * ??    回：void 
* ??    ??：单??设定y轴的速度??
******************************************************************************/
void sdk_velocity_y_set(float y)
{
  pc_user.vel_cmps_set_h[1] = LIMIT(y, -MAX_VELOCITY_Y, MAX_VELOCITY_Y);
  pc_user.vel_cmps_set_h[1]=Rotation*pc_user.vel_cmps_set_h[1];
}

/******************************************************************************
  * 函数名称：sdk_init
  * 函数描述：sdk的初始化
  * ??    入：void
  * ??    出：void
  * ??    回：void 
  * ??    ??：null    
  *    
  *
******************************************************************************/

enum{
  East_North=0,
  West_North,
  East_South,
  West_South
};
uint8_t Coordinate_Type;

typedef struct 
{
	float in_last;
	float a;
}LPF_Val;
	
	
float LPF(LPF_Val lpf,float input)
{
if( !(lpf.a>0&&lpf.a<1)){
//	#error [please input a reasonable input]
}
float out=input*lpf.a+lpf.in_last*(1-lpf.a);
lpf.in_last=out;
return out;
}

LPF_Val X_LPF;
LPF_Val Y_LPF;
float angle_first;

void sdk_init()
{
	X_LPF.a=0.95;
	Y_LPF.a=0.95;
	
PID_X.kp=0.2;
PID_X.ki=0;
PID_X.kd_ex=0.1;
PID_X.fb_d_mode=1;

PID_Y.kp=0.2;
PID_Y.ki=0;
PID_Y.kd_ex=0.1;
PID_Y.fb_d_mode=1;
	
PID_H.kp=0.5;
PID_H.ki=0.1;
PID_H.kd_ex=0;

angle_first=att_2l_ct.fb_yaw;//初始化时候的绝对角度，罗盘返回的是（真）世界坐标系（地球磁极）的绝对值
	
Coordinate_Type=East_North;
	Position_Init();
}


void sdk_pos_update()
{

  sdk_manager.coordinate_x=(int16_t)LPF(X_LPF,T265.coordinate_x);
  sdk_manager.coordinate_y=(int16_t)LPF(Y_LPF,T265.coordinate_y);

}
/******************************************************************************
 * 函数名称：sdk_pos_set(float x,float y)
* 函数描述：水平的坐标????
* ??    入：x方向??的坐标，y方向??的坐??
 * ??    出：void
 * ??    回：void 
* ??    ??：void
******************************************************************************/

uint8_t sdk_pos_set(int16_t x,int16_t y)//水平位置pid
{
  sdk_pos_update();//位置更新以及滤波

	PID_calculate(    0.001,            //周期（单位：秒）
										0 ,				//前馈值x
										x ,				//期望值（设定值）
										sdk_manager.coordinate_x,			//反馈值（）y
										&PID_X, //PID参数结构体
										&PID_X_Val,	//PID数据结构体
	                  0.1,//积分误差限幅
										5 *flag.taking_off			//integration limit，积分限幅
										 )	;
  PID_calculate(    0.001,            //周期（单位：秒）
                    0 ,				//前馈值x
                    y ,				//期望值（设定值）
                    sdk_manager.coordinate_y,			//反馈值（）y
                    &PID_Y, //PID参数结构体
                    &PID_Y_Val,	//PID数据结构体
                    0.1,//积分误差限幅
                    5 *flag.taking_off			//integration limit，积分限幅
    )	;


    sdk_velocity_x_set(PID_X_Val.out);sdk_velocity_y_set(PID_Y_Val.out);
    //位置环输出作为速度环的目标
  //坐标转换
 if( (fabs(PID_Y_Val.err)<ERR_LIMIT) && (fabs(PID_X_Val.err)<ERR_LIMIT) )
 {return 1;}
return 0;
}

uint8_t sdk_pos_set_x(int16_t x)//水平位置pid
{
  sdk_pos_update();//位置更新以及滤波

	PID_calculate(    0.001,            //周期（单位：秒）
										0 ,				//前馈值x
										x ,				//期望值（设定值）
										sdk_manager.coordinate_x,			//反馈值（）y
										&PID_X, //PID参数结构体
										&PID_X_Val,	//PID数据结构体
	                  5,//积分误差限幅
										5 *flag.taking_off			//integration limit，积分限幅
										 )	;
    sdk_velocity_x_set(PID_X_Val.out);
    //位置环输出作为速度环的目标
  //坐标转换
if(fabs(PID_X_Val.err)<ERR_LIMIT)
 {return 1;}
return 0;
}




uint8_t sdk_pos_set_y(int16_t y)//水平位置pid
{
  sdk_pos_update();//位置更新以及滤波

  PID_calculate(    0.001,            //周期（单位：秒）
                    0 ,				//前馈值x
                    y ,				//期望值（设定值）
                    sdk_manager.coordinate_y,			//反馈值（）y
                    &PID_Y, //PID参数结构体
                    &PID_Y_Val,	//PID数据结构体
                    5,//积分误差限幅
                    5 *flag.taking_off			//integration limit，积分限幅
    )	;


    sdk_velocity_x_set(PID_X_Val.out);sdk_velocity_y_set(PID_Y_Val.out);
    //位置环输出作为速度环的目标
  //坐标转换
 if(fabs(PID_Y_Val.err)<ERR_LIMIT)
 {return 1;}
return 0;
}


/******************************************************************************
  * 函数名称：sdk_takeoff
  * 函数描述：使用sdk进???无人机的起飞工作，包含??动解锁等内???
  * ??    入：alitude  ??动起飞后的??????高??
  * ??    出：void
  * ??    回：void 
  * ??    ??：注意安??，谨慎使??    
  *    
  *
******************************************************************************/
void sdk_takeoff(float alititude)
{
	Ano_Parame.set.auto_take_off_speed=50;
  Ano_Parame.set.auto_take_off_height=alititude;
  one_key_take_off();
}

/******************************************************************************
  * 函数名称：sdk_land
  * 函数描述：使用sdk进???无人机的降落工作，包含??动上锁等内???
  * ??    入：void
  * ??    出：void
  * ??    回：void 
  * ??    ??：注意安??，谨慎使??    
  *    
  *
******************************************************************************/
void sdk_land()
{
    one_key_land();
}

void sdk_reset_Location()
{
   sdk_manager.coordinate_x = 0;
   sdk_manager.coordinate_y = 0;
}



/******************************************************************************
 * 函数名称：sdk_alititude_set
 * 函数描述：使用sdk??改期望高度??
 * ??    入：void
 * ??    出：void
 * ??    回：void 
   * ??    ??：暂不开??
 *    
 *
******************************************************************************/

bool sdk_alititude_set(float altitude)
{
  PID_calculate(    0.001,            //周期（单位：秒）
    0 ,				//前馈值x
    altitude ,				//期望值（设定值）
    loc_ctrl_2.fb[Z],			//反馈值（）y
    &PID_H, //PID参数结构体
    &PID_H_Val,	//PID数据结构体
    5,//积分误差限幅
    25 *flag.taking_off			//integration limit，积分限幅
)	;
if(ABS(PID_H_Val.err)>= 1) {Program_Ctrl_User_Set_Zcmps(PID_H_Val.out); return false;} 
else  return true;
  // if(altitude-loc_ctrl_2.fb[Z]>5){Program_Ctrl_User_Set_Zcmps(30);}
  // else if(altitude - loc_ctrl_2.fb[Z]<-5){Program_Ctrl_User_Set_Zcmps(-30);}
  // else{Program_Ctrl_User_Set_Zcmps(0);return true;}
  //  return false;
}



/******************************************************************************
 * 函数名称：sdk_velocity_set
 * 函数描述：使用sdk??改期望水平速度期望??
 * ??    入：float x float y表示水平速度方向和大??
 * ??    出：void
 * ??    回：void 
   * ??    ??：速度参考方向可以按照???图来进行，一??来???，速度绝???值???定??20即可完成大部分程控赛??
 *
 *
      (+X)
       |
       |
(-Y)---------(+Y)
       |
       |
      (-X)

******************************************************************************/
void sdk_velocity_set(float x, float y)
{
   pc_user.vel_cmps_set_h[0] = LIMIT(x, -MAX_VELOCITY_X, MAX_VELOCITY_X);
   pc_user.vel_cmps_set_h[1] = LIMIT(y, -MAX_VELOCITY_Y, MAX_VELOCITY_Y);
}

/******************************************************************************
 * 函数名称：sdk_lock
* 函数描述：使用sdk对无人机进???上??
 * ??    入：void
 * ??    出：void
 * ??    回：void 
 * ??    ??：void
******************************************************************************/
void sdk_lock()
{
   //上锁
   flag.unlock_sta=0;
}

/******************************************************************************
 * 函数名称：sdk_unlock
* 函数描述：使用sdk对无人机进???解??
 * ??    入：void
 * ??    出：void
 * ??    回：void 
 * ??    ??：void
******************************************************************************/
void sdk_unlock()
{
   //解锁
   flag.unlock_sta = 1;
}

//******************************************************************************
//  * 函数名称：sdk_yaw_set
//* 函数描述：使用sdk对无人机进???转角??????
//  * ??    入：void
//  * ??    出：void
//  * ??    回：void 
//  * ??    ??：void
//******************************************************************************/
uint8_t FirstTime=1;
float angle_fix;
float overd_yaw;
u8 k=0;
float fb_yaw_last=0;
bool sdk_yaw_set(float angle)
{	
	if(FirstTime){
		angle_first=att_2l_ct.fb_yaw;//刚进入转向环节时候的绝对角度，罗盘返回的是（真）世界坐标系（地球磁极）的绝对值
		FirstTime=0;
	}
	if (fb_yaw_last < -90 && att_2l_ct.fb_yaw > 90) {k -= 1;}
	if (fb_yaw_last > 90 && att_2l_ct.fb_yaw < -90) { k += 1;}
	fb_yaw_last = att_2l_ct.fb_yaw;
	overd_yaw = att_2l_ct.fb_yaw + (float)k * 360 -angle_first;
	
	  if(angle-overd_yaw>5){Program_Ctrl_User_Set_YAWdps(20);}
	  else if(angle-overd_yaw<-5){Program_Ctrl_User_Set_YAWdps(-20);}
	  else{Program_Ctrl_User_Set_YAWdps(0);return true;}
   return false;
}

///******************************************************************************
//  * 函数名称：sdk_round_set
//  * 函数描述：使用sdk对围绕某一半径做圆形运??，相当于绕杆飞???，飞???时，无人机机头对准??
//  * ??    入：float distance, float angle, uint8_t R_
//    distance：表示圆形运动的半径
//    float angle：表示运行的角度
//    uint8_t R_：表示左??还是右转，???果R大于0，则??视下无人机绕杆逆时针旋??，???果R小于0，则??视下无人机绕杆顺时针旋转
//  * ??    出：void
//  * ??    回：void 
//  * ??    ??：void
//******************************************************************************/
//void sdk_round_set(float distance, float angle, int R_)
//{
//    /*
//    distance实际上是这个圈的半径，我需要???算相关的信??
//    t = angle / d_angle;
//    v_r = 2 * pi * distance * angle / 360 / t

//    eg :distance 50 cm
//    angle:360

//    0.2 = 角速度  ??10ms + 0.2  =  1s + 20°

//    t = 360 / 20 = 360 / 20 = 18s;
//    v_r = 2 * 3.1415926 * 50 * 360 / 360 / 18 = 17.453;
//    */
//    float pi = 3.1415926f;
//    float t = 0;
//    t = angle / (sdk_manager.sdk_yaw_d_angle * 100);
//    
//    
//    if (R_ > 0)
//    {
//        sdk_manager.sdk_velocity_y = 2 * pi * distance * angle / 360 / t;
//    }
//    else
//    {
//        sdk_manager.sdk_velocity_y = 2 * pi * distance * angle / 360 / t * -1;
//    }

//    sdk_yaw_set(angle);
//}



///******************************************************************************
//  * 函数名称：is_yaw_set_compleate
//* 函数描述：确认是??YAW轴的??动已经结??
//* ??    入：void
//  * ??    出：void
//  * ??    回：void 
//* ??    ??：yaw的转动需要时间，此函数用以确??yaw的转动是否结??
//******************************************************************************/
//bool is_yaw_set_compleate()
//{
//    bool result = false;

//    if (sdk_manager.sdk_yaw_step != 0)
//    {
//        result = true;
//    }

//    return result;
//}


//bool yaw_round_180()
//{
//sdk_yaw_set(180);
//Rotation=-Rotation;
//return 0;
//}
///******************************************************************************
//  * 函数名称：sdk_yaw_reset
//* 函数描述：重置yaw??
//* ??    入：void
//  * ??    出：void
//  * ??    回：void 
//* ??    ??：相当于让???机重新归位
//******************************************************************************/
//void sdk_yaw_reset()
//{
//    PIDGroup_desired_yaw_pos_tmp = 0;
//}

///******************************************************************************
//  * 函数名称：sdk_yaw_stop
//* 函数描述：停??yaw角转??
//* ??    入：void
//  * ??    出：void
//  * ??    回：void 
//* ??    ??：相当于让???机重新归位
//******************************************************************************/
//void sdk_yaw_stop()
//{
//    sdk_manager.sdk_yaw_step = 1;
//    sdk_manager.sdk_yaw_angle_count = 0;
//}

///**
//  * 函数功能：卡尔曼滤波Roll
//  * 入口参数：Angle_a：加速度Roll
//  * 入口参数：Angle_g：角速度Roll
//  * 返 回 值：无
//  */
//float Kalman_Filter(float Angle_a, float Angle_g)
//{
//    static float Angle;              //最终角度
//    
//    float dt      = 0.01;    //采样周期即计算任务周期10ms
//    
//    float Q_Angle = 0.002;   //角度数据置信度，角度噪声的协方差
//    float Q_Gyro  = 0.001;   //角速度数据置信度，角速度噪声协方差
//    float R_Angle = 0.5;     //加速度计测量噪声的协方差
//    
//    float Q_Bias;            //上次最优估计值偏差
//    float Angle_err;         //角度偏差
//    float K_0;               //用于计算最优估计值
//    float K_1;               //用于计算最优估计值的偏差
//    
//    static float P[2][2] = { {1, 0}, {0, 1} };   //过程协方差矩阵P，初始值为单位阵
//    
// 
//    /*Step1: 先验估计*/
//    /* 状态方程,角度值等于上次最优角度加角速度减零漂后积分 */
//    Angle += (Angle_g - Q_Bias) * dt;          
//    
//    /*Step2: 预测协方差矩阵  */
//    P[0][0] += ( Q_Angle - P[0][1] - P[1][0] * dt );
//    P[0][1] += -P[1][1] * dt;
//    P[1][0] += -P[1][1] * dt;
//    P[1][1] += Q_Gyro * dt; 
//    
//    /*Step3: 计算卡尔曼增益  */
//    K_0 = P[0][0] / ( R_Angle + P[0][0] );
//    K_1 = P[0][1] / ( R_Angle + P[0][0] );
//    
//    /*Step4: 更新协方差矩阵 */
//    P[0][0] -= K_0 * P[0][0];
//    P[0][1] -= K_0 * P[0][1];
//    P[1][0] -= K_1 * P[0][0];
//    P[1][1] -= K_1 * P[0][1];
//    
//    /*Step5: 计算最优角度值 */
//    Angle_err = Angle_a - Angle;      //先验估计，计算角度偏差
//    Angle     += K_0 * Angle_err;     //后验估计，得到最优估计值
//    Q_Bias   += K_1 * Angle_err;     //后验估计，得到最优估计偏差
//    
//    return Angle;
//}

///******************************************************************************
//  * 函数名称：sdk_yaw_little
//* 函数描述：??????机旋转??小的yaw角，适用??0.5度之??
//* ??    入：void
//  * ??    出：void
//  * ??    回：void 
//* ??    ??：void
//******************************************************************************/
//extern float YAW_First;
//void sdk_yaw_little(float yaw)
//{
//    //注意，这里的YAW取值范围需要在-5 5之间
//    yaw = LIMIT(yaw, -0.5f, 0.5f);
//    PIDGroup_desired_yaw_pos_tmp += yaw;
//}

//void sdk_yaw_target(float yaw)
//{
//    //注意，这里的YAW取值范围需要在-5 5之间
//    yaw = LIMIT(yaw, -0.5f, 0.5f);
//    PIDGroup_desired_yaw_pos_tmp =YAW_First+yaw;
//}

//#define UWB//使用UWB

//void sdk_reset_position()
//{
//	#ifdef UWB
//	#else
//    sdk_manager.location_x = 0;
//    sdk_manager.location_y = 0;
//	#endif
//}


//extern int16_t coordinate_x,coordinate_y,coordinate_z;
//extern int16_t coordinate_x_last,coordinate_y_last;
//float speed_x_last,speed_y_last=0;

//void Mixed_Data_K()
//{
//  float uwb_speed_x=coordinate_x-coordinate_x_last;
//  float uwb_speed_y=coordinate_y-coordinate_y_last;
//	float  Mix_displacement_x,Mix_displacement_y=0;
//	
//Mix_displacement_x=Kalman_Filter(uwb_speed_x,ANO_OF.DX2FIX * 0.01f);
//Mix_displacement_y=Kalman_Filter(uwb_speed_y,ANO_OF.DY2FIX * 0.01f);//卡尔曼滤波方案

//	sdk_manager.location_x=coordinate_x_last+Mix_displacement_x;
//	sdk_manager.location_y=coordinate_y_last+Mix_displacement_y;
//	coordinate_x_last=sdk_manager.location_x;
//	coordinate_y_last=sdk_manager.location_y;
//	
//}

//void Mixed_Data()
//{
//  float uwb_speed_x=coordinate_x-coordinate_x_last;
//  float uwb_speed_y=coordinate_y-coordinate_y_last;
//	float  Mix_displacement_x,Mix_displacement_y=0;

//	
//	if(ANO_OF.DX2FIX > -100 && ANO_OF.DX2FIX < 100 && 
//     ANO_OF.DY2FIX > -100 && ANO_OF.DY2FIX < 100		)
//    {
//			if( 
//					(ANO_OF.DX2FIX-speed_x_last<30) && (ANO_OF.DY2FIX-speed_y_last<30)&&
//					(ANO_OF.DX2FIX-speed_x_last>-30) && (ANO_OF.DY2FIX-speed_y_last>-30)
//				)
//			{
//	  Mix_displacement_x=uwb_speed_x*0.9f + ANO_OF.DX2FIX * 0.001f;
//	  Mix_displacement_y=uwb_speed_y*0.9f + ANO_OF.DY2FIX * 0.001f;
//			}
//			else{
//		Mix_displacement_x=uwb_speed_x;
//		Mix_displacement_y=uwb_speed_y;
//		}
//			speed_x_last=ANO_OF.DX2FIX;speed_y_last=ANO_OF.DY2FIX;//测量速度的变化值如果太大则不进行融合
//    }
//		else{
//		Mix_displacement_x=uwb_speed_x;
//		Mix_displacement_y=uwb_speed_y;
//		}

//	sdk_manager.location_x=coordinate_x_last+Mix_displacement_x;
//	sdk_manager.location_y=coordinate_y_last+Mix_displacement_y;
//	coordinate_x_last=sdk_manager.location_x;
//	coordinate_y_last=sdk_manager.location_y;
//}

//void sdk_update_position()//更新位置
//{
//	#ifdef UWB
//	sdk_manager.location_x=coordinate_x;
//	sdk_manager.location_y=coordinate_y;
//	#else
//    if(ANO_OF.DX2FIX > -100 && ANO_OF.DX2FIX < 100 && 
//       ANO_OF.DY2FIX > -100 && ANO_OF.DY2FIX < 100)
//    {
//        sdk_manager.location_x += ANO_OF.DX2FIX * 0.01f;
//        sdk_manager.location_y += ANO_OF.DY2FIX * 0.01f;
//    }
//		#endif
//		
//}



//#include "math.h"
//#include "stdlib.h"
//bool Hold_On_Succesed(int Cx,int Tx,int time,uint16_t Err)
//{
//uint16_t err=abs(Cx-Tx);
//	static int i=0;
//	if(err<Err)i++;else{i=0;}
//	if(i>=time){i=0;return 1;}
//	return 0;
//}


//void sdk_pos_set_x(float x)//只有x方向的水平位置环
//{
//    sdk_pid[sdk_pos_x].measured = sdk_manager.location_x;
//    sdk_pid[sdk_pos_x].desired = x;
//    UpdatePID(&sdk_pid[sdk_pos_x],0.01f);//位置??
//    sdk_velocity_x_set(sdk_pid[sdk_pos_x].out);//位置??输出作为速度??的目??

//}

//void sdk_pos_set_y(float y)//只有y方向水平位置环
//{
//    sdk_pid[sdk_pos_y].measured = sdk_manager.location_y;
//    sdk_pid[sdk_pos_y].desired = y;
//    UpdatePID(&sdk_pid[sdk_pos_y],0.01f);
//    sdk_velocity_y_set(sdk_pid[sdk_pos_y].out);

//}

//bool is_pos_x_set_compleate(float max_err)
//{
//    bool result = false;
//    
//    if(ABS(sdk_pid[sdk_pos_x].Err) < max_err)
//    {
//        result = true;
//    }
//    
//    return result;
//}

//bool is_pos_y_set_compleate(float max_err)
//{
//    bool result = false;
//    
//    if(ABS(sdk_pid[sdk_pos_y].Err) < max_err)
//    {
//        result = true;
//    }
//    
//    return result;
//}

//bool is_pos_set_compleate(float max_err)//检测是否达到目标位??
//{
//    bool result = false;
//    
//    if(    is_pos_y_set_compleate(max_err) 
//        && is_pos_x_set_compleate(max_err))
//    {
//        result = true;
//    }
//    
//    return result;
//}

///*
//       |cos(θ)   -sin(θ)|
//M(θ) = |                |
//       |sin(θ)    cos(θ)|
//    |x|
//a = | |
//    |y|
//            |x * cos(θ) + y * sin(θ)|
//b = a * M = |                       |
//            |x * -sin(θ)+ y * cos(θ)|

//二维旋转矩阵
//* 函数名称：RotateLocation
//* 函数描述：在没有偏移的情况下，水平旋??某一坐标
//* ??    入：int16_t *input 输入的坐标地址
//            float xita 需要旋??的???度
//* ??    出：int16_t *output 输出的坐标地址
//* ??    回：void 
//* ??    ??：void
//*/

//void RotateLocation(int16_t *input, int16_t *output, float xita)
//{
//    float cosdata = cosf(xita / 57.3f);
//    float sindata = sinf(xita / 57.3f);
//    output[0] = input[0] * cosdata + input[1] * sindata;
//    output[1] = input[0] * -sindata + input[1] * cosdata;
//}


//void sdk_update_s_2(float raw, s2_t *s2)
//{
//    float sum = 0;
//    float avg = 0;
//    
////    memmove(s2->data_temp + 1, s2->data_temp, s2->length - 1);
//    
//    for(int i = 1;i < s2->length;i++)
//    {
//        s2->data_temp[i] = s2->data_temp[i - 1];
//    }
//    s2->data_temp[0] = raw;
//    
//    //计算avg
//    for(int i = 0;i < s2->length;i++)
//    {
//        sum += s2->data_temp[i];
//    }
//    
//    avg = sum / s2->length;
//    
//    //计算s2
//    sum = 0;
//    for(int i = 0;i < s2->length;i++)
//    {
//        sum += squa(s2->data_temp[i] - avg);
//    }
//    
//    s2->s2 = sum / (s2->length - 1);
//}





